#这段代码实现了基于 OpenMV 摄像头的多种视觉功能，适用于智能物流搬运任务的不同场景。它通过 UART 与主控系统通信，支持：

#颜色块识别（红、绿、蓝）
#直线检测与物体校正
#圆形放置位检测
#二维码扫描
import pyb
import time
import image
import sensor
from pyb import UART
enable_lens_corr = False
BINARY_VISIBLE = True
sensor.reset()
sensor.set_pixformat(sensor.RGB565)  # 灰度更快
sensor.set_framesize(sensor.QVGA)
sensor.skip_frames(time=2000)
sensor.set_auto_exposure(False)
sensor.set_auto_whitebal(False)
change_threshold = (0, 74, -128, 127, -128, -15)
green_threshold = (0, 100, -5, 127, -128, -18)
green_threshold_2 = (55, 86, -66, -22, -26, 38)
black_threshold = (30, 0, 26, -20, 19, -16)
red = [(22, 80, 35, 77, -50, 22)]
green = [(60, 88, -56, -29, -26, 7)]
blue = [(47, 71, -27, 4, -71, -31)]
sensor.set_auto_gain(0)  # 颜色跟踪必须关闭自动增益
sensor.set_auto_whitebal(0)  # 颜色跟踪必须关闭白平衡
min_degree = 0
max_degree = 179
op = 0
ddd = 0
clock = time.clock()
uart = UART(3, 115200)
pyb.LED(1).on()
pyb.LED(2).on()
pyb.LED(3).on()
min_degree = 0
max_degree = 179
i = 0
j = 0
k = 0
p = 0
q = 1  # 进转盘颜色识别
r_mod = 0
g_mod = 0
b_mod = 0
# EHx1=0
# EHy1=0
# EHx2=0
# EHy2=0
# ESx1=0
# ESy1=0
# ESx2=0
# ESy2=0
Hx1 = [0]*15
Hx2 = [0]*15
Hy1 = [0]*15
Hy2 = [0]*15
cgx = 0
cgy = 0
EHx1 = [0]*15
EHy1 = [0]*15
EHx2 = [0]*15
EHy2 = [0]*15
ESx1 = [0]*15
ESy1 = [0]*15
ESx2 = [0]*15
ESy2 = [0]*15
give_up_r = 0
give_up_g = 0
give_up_b = 0


def median(data, length):
    sorted_data = sorted(data, reverse=True)
    print(sorted_data)
    if length % 2 == 1:
        median = sorted_data[length // 2]
    else:
        median = (sorted_data[length // 2 - 1] + sorted_data[length // 2]) / 2
    return median


while (True):
    read_data = uart.read()
    clock.tick()
    if read_data:

        read_str = read_data.decode('utf-8')
        print("string = ", read_str)
        if read_str == 'wcop':
            op = 1
            r_mod = 0
            g_mod = 0
            b_mod = 0
            give_up_r = 0
            give_up_g = 0
            give_up_b = 0
            print(op)
            sensor.set_auto_exposure(True)

        # 底边矫正
        if read_str == 'wczp':
            op = 2
            sensor.set_auto_exposure(True)
            print(op)

        # 圆形放置位矫正
        if read_str == 'wcnp':
            op = 3
            sensor.set_auto_exposure(True)
            print(op)

        # 关
        if read_str == 'wcsb':
            op = 0
            print(op)
            sensor.set_auto_exposure(False)
        # 欢乐大转盘识别
        if read_str == 'wcqg':
            op = 4
            print(op)
            q = 1

            sensor.set_auto_exposure(True)
        if read_str == 'wctt':  # 物块矫正
            op = 5
            sensor.set_auto_exposure(True)
            print(op)

        if read_str == 'wczm':  # 新增二维码扫描命令
            op = 6
            print(op)

    if op == 4 or op == 3 or op == 5:
        sensor.set_pixformat(sensor.RGB565)
    if op != 1 and op != 2:
        sensor.set_framesize(sensor.QVGA)
        img = sensor.snapshot()
    if op == 1 or op == 2:
        #        ddd = 1
        sensor.set_framesize(sensor.QQVGA)
    if op == 1:
        # sensor.set_pixformat(sensor.GRAYSCALE)
        img = sensor.snapshot()
#        img.mean(1)
#        img.binary([change_threshold])
#        img.mean(2)
#        for l in img.find_lines(threshold = 2000, theta_margin = 0, rho_margin = 0):
#            if (min_degree <= l.theta()) and (l.theta() <= max_degree):
#                img.draw_line(l.line(), color = (255, 0, 0))
        lines = img.find_line_segments(merge_distance=0, max_theta_diff=80)
        img.clear()
        for l in lines:
            #        if(ddd == 0):

            img.draw_line(l.line(), color=(255, 255, 255), thickness=2)
        line1 = img.get_regression(
            [(29, 100, -128, 127, -128, 127)], roi=(0, 0, 30, 120))
        line3 = img.get_regression(
            [(29, 100, -128, 127, -128, 127)], roi=(30, 0, 120, 30))
        if (line1):
            img.draw_line(line1.line(), color=(0, 0, 255))
        if (line3):
            img.draw_line(line3.line(), color=(0, 0, 255))
#        print(line1)
#        print(line3)
        if i < 5 and j < 5:
            if (line1):
                EHx1[i] = line1.x1()
                EHy1[i] = line1.y1()
                EHx2[i] = line1.x2()
                EHy2[i] = line1.y2()

                i = i+1
#                print("i=%d" % i)
        # else:
            if (line3):
                ESx1[j] = line3.x1()
                ESy1[j] = line3.y1()
                ESx2[j] = line3.x2()
                ESy2[j] = line3.y2()
                j = j+1
#                print("j=%d" % j)
        else:
            ZEHx1 = median(EHx1, i)
            ZEHy1 = median(EHy1, i)
            ZEHx2 = median(EHx2, i)
            ZEHy2 = median(EHy2, i)
            ZESx1 = median(ESx1, j)
            ZESy1 = median(ESy1, j)
            ZESx2 = median(ESx2, j)
            ZESy2 = median(ESy2, j)
            print("%d" % ZEHx1)
            print("%d" % ZEHy1)
            print("%d" % ZEHx2)
            print("%d" % ZEHy2)
            uart.writechar(0X48)
            uart.writechar(int(ZEHx1))
            uart.writechar(int(ZEHx2))
            uart.writechar(int(ZEHy1))
            uart.writechar(int(ZEHy2))
            uart.writechar(0XBB)
            print("%d" % ZESx1)
            print("%d" % ZESy1)
            print("%d" % ZESx2)
            print("%d" % ZESy2)
            # print("%d" % EHx1)
            # print("%d" % EHy1)
            # print("%d" % EHx2)
            # print("%d" % EHy2)
            uart.writechar(0X53)
            uart.writechar(int(ZESx1))
            uart.writechar(int(ZESx2))
            uart.writechar(int(ZESy1))
            uart.writechar(int(ZESy2))
            uart.writechar(0XBB)
            EHx1 = [0]*15
            EHy1 = [0]*15
            EHx2 = [0]*15
            EHy2 = [0]*15
            ESx1 = [0]*15
            ESy1 = [0]*15
            ESx2 = [0]*15
            ESy2 = [0]*15
            print("i=%d" % i)
            print("j=%d" % j)
            i = 0
            j = 0
            print("FPS %f" % clock.fps())
    if op == 2:
        img = sensor.snapshot()
#        img.binary([change_threshold])
#        img.mean(1)
#        for l in img.find_lines(threshold = 3000, theta_margin = 0, rho_margin = 0):
#            if (min_degree <= l.theta()) and (l.theta() <= max_degree):
#                img.draw_line(l.line(), color = (255, 0, 0))
        lines2 = img.find_line_segments(merge_distance=0, max_theta_diff=80)
        img.clear()
        for l in lines2:
            img.draw_line(l.line(), color=(255, 255, 255), thickness=2)
        line2 = img.get_regression(
            [(29, 100, -128, 127, -128, 127)], roi=(0, 0, 40, 120))
        line4 = img.get_regression(
            [(29, 100, -128, 127, -128, 127)], roi=(120, 0, 40, 120))
        if (line2):
            img.draw_line(line2.line(), color=(0, 0, 255))
        if (line4):
            img.draw_line(line4.line(), color=(0, 0, 255))
        if (line2 and line4):
            if k <= 3:
                Hx1[k] = line2.x1()
                Hy1[k] = line2.y1()
                Hx2[k] = line4.x2()
                Hy2[k] = line4.y2()
                k = k + 1
            else:
                ZHx1 = median(Hx1, k)
                ZHy1 = median(Hy1, k)
                ZHx2 = median(Hx2, k)
                ZHy2 = median(Hy2, k)
                uart.writechar(0X48)
                uart.writechar(int(ZHx1))
                uart.writechar(int(ZHx2))
                uart.writechar(int((120-ZHy2)))
                uart.writechar(int((120-ZHy1)))

                uart.writechar(0XBB)
                k = 0
                ZHx1 = 0
                ZHx2 = 0
                ZHy1 = 0
                ZHy2 = 0
                Hx1 = [0]*15
                Hy1 = [0]*15
                Hx2 = [0]*15
                Hy2 = [0]*15
                print("FPS %f" % clock.fps())
    if op == 3:
        img.binary([green_threshold])
        for c in img.find_circles(threshold=12000, x_margin=100, y_margin=100, r_margin=100, r_min=45, r_max=50, r_step=2):
            img.draw_circle(c.x(), c.y(), c.r(), color=(255, 0, 0))
            if p <= 3:
                cgy = cgy+c.y()
                cgx = cgx+c.x()
                p = p + 1
            else:
                cgx = cgx/p
                cgy = cgy/p
                uart.writechar(0X52)
                uart.writechar(int(cgx/2))
                uart.writechar(int(cgy/2))
                uart.writechar(0XBB)
                print(int(cgx/2))
                print(int(cgy/2))
                cgx = 0
                cgy = 0
                p = 0
                print(c.r())

                print("FPS %f" % clock.fps())
    if op == 5:
        img.binary([green_threshold_2])
        for c in img.find_circles(threshold=12000, x_margin=100, y_margin=100, r_margin=100, r_min=61, r_max=67, r_step=2):
            img.draw_circle(c.x(), c.y(), c.r(), color=(255, 0, 0))
            if p <= 1:
                cgy = cgy+c.y()
                cgx = cgx+c.x()
                p = p + 1
            else:
                cgx = cgx/p
                cgy = cgy/p
                uart.writechar(0X52)
                uart.writechar(int(cgx/2))
                uart.writechar(int(cgy/2))
                uart.writechar(0XBB)
                cgx = 0
                cgy = 0
                p = 0
                print(c.r())

    if op == 6:  # 二维码扫描模式
        img = sensor.snapshot()
        for code in img.find_qrcodes():
            print("QR Code Content:", code.payload())  # 打印二维码内容
            uart.write(f"QR:{code.payload()}\r\n")  # 将二维码内容通过 UART 发送
            # 可根据二维码内容执行相应逻辑
            if code.payload() == 'pickup':
                uart.write("start_pickup\r\n")  # 发送指令给控制系统



    if op == 4:
        print("sb")
        # 寻找对应阈值的色块，阈值小于300像素的色块过滤掉，合并相邻像素在10个像素内的色块
        blob = img.find_blobs(red, area_threshold=10000, margin=10)
        if blob and q == 1 and r_mod == 0:  # 如果找到了目标颜色
            FH = bytearray([0xb3, 0xb4])
            for b in blob:
                # 迭代找到的目标颜色区域
                img.draw_cross(b[5], b[6])  # 画十字 cx,cy
    #            blob.cx() 返回色块的外框的中心x坐标（int），也可以通过blob[5]来获取。
    #            blob.cy() 返回色块的外框的中心y坐标（int），也可以通过blob[6]来获取。
                img.draw_edges(b.min_corners(), color=(0, 255, 0))  # 画框
                give_up_r = 1
                if give_up_r == 1 and give_up_g == 0 and give_up_b == 0:
                    give_up_r = 1
                else:
                    uart.writechar(0X41)
                    uart.writechar(0X52)
                    uart.writechar(0XBB)
                    print("red")
                # q = 0
                # r_mod = 1
        # 寻找对应阈值的色块，阈值小于300像素的色块过滤掉，合并相邻像素在10个像素内的色块
        blob = img.find_blobs(green, area_threshold=10000, margin=10)
        if blob and q == 1 and g_mod == 0:  # 如果找到了目标颜色
            FH = bytearray([0xb3, 0xb4])
            for b in blob:
                # 迭代找到的目标颜色区域
                img.draw_cross(b[5], b[6])  # 画十字 cx,cy
    #            blob.cx() 返回色块的外框的中心x坐标（int），也可以通过blob[5]来获取。
    #            blob.cy() 返回色块的外框的中心y坐标（int），也可以通过blob[6]来获取。
                img.draw_edges(b.min_corners(), color=(0, 255, 0))  # 画框
                give_up_g = 1
                if give_up_r == 0 and give_up_g == 1 and give_up_b == 0:
                    give_up_g = 1
                else:
                    uart.writechar(0X41)
                    uart.writechar(0X47)
                    uart.writechar(0XBB)
                    print("green")
                # q = 0
                # g_mod = 1
        # 寻找对应阈值的色块，阈值小于300像素的色块过滤掉，合并相邻像素在10个像素内的色块
        blob = img.find_blobs(blue, area_threshold=12000, margin=10)
        if blob and q == 1 and b_mod == 0:  # 如果找到了目标颜色
            FH = bytearray([0xb3, 0xb4])
            for b in blob:
                # 迭代找到的目标颜色区域
                img.draw_cross(b[5], b[6])  # 画十字 cx,cy
    #            blob.cx() 返回色块的外框的中心x坐标（int），也可以通过blob[5]来获取。
    #            blob.cy() 返回色块的外框的中心y坐标（int），也可以通过blob[6]来获取。
                img.draw_edges(b.min_corners(), color=(0, 255, 0))  # 画框
                give_up_b = 1
                if give_up_r == 0 and give_up_g == 0 and give_up_b == 1:
                    give_up_b = 1
                else:
                    uart.writechar(0X41)
                    uart.writechar(0X42)
                    uart.writechar(0XBB)
                    print("blue")
                    # q = 0
                    # b_mod = 1



